//
// Created by ZhaoXiaoFei on 2022/10/24.
// Last changed by: ZhaoXiaoFei.
// Last update: 2022/10/24.
//

#ifndef MSCKF_MONO_DATA_TYPES_H
#define MSCKF_MONO_DATA_TYPES_H
#include <Eigen/Dense>

inline Eigen::Matrix3f vectorToSkewSymmetric(const Eigen::Vector3f& Vec) {
    // Returns skew-symmetric form of a 3-d vector
    Eigen::Matrix3f M;

    M << 0, -Vec(2), Vec(1),
            Vec(2), 0, -Vec(0),
            -Vec(1), Vec(0), 0;
    return M;
}

inline Eigen::Matrix4f omegaMat(const Eigen::Vector3f & omega) {
    // Compute the omega-matrix of a 3-d vector omega
    Eigen::Matrix4f bigOmega;
    bigOmega.setZero();

    bigOmega.template block<3, 3>(0, 0) = -vectorToSkewSymmetric(omega);
    bigOmega.template block<3, 1>(0, 3) = omega;
    bigOmega.template block<1, 3>(3, 0) = -omega.transpose();

    return bigOmega;
}

// Slice columns and rows at indices inds from in to out
//
// in - M x M
// inds - N x 1
// out - N x N
template <typename _Scalar>
inline void square_slice(const Eigen::MatrixX<_Scalar>& in,
                         const Eigen::VectorXi& inds,
                         Eigen::MatrixX<_Scalar>& out ){
    int inds_size = inds.rows();
    out.resize(inds_size, inds_size);
    for(int i=0; i<inds_size; i++){
        for(int j=0; j<inds_size; j++){
            out(i,j) = in(inds(i),inds(j));
        }
    }
}

// Slice columns at indices inds from in to out
//
// in - R x M
// inds - N x 1
// out - R x N
template <typename _Scalar, int _Rows>
inline void column_slice(const Eigen::Matrix<_Scalar, _Rows, Eigen::Dynamic>& in,
                         const Eigen::VectorXi& inds,
                         Eigen::Matrix<_Scalar, _Rows, Eigen::Dynamic>& out){
    int inds_size = inds.rows();
    int rows = in.rows();
    out.resize(rows, inds_size);
    for(int i=0; i<rows; i++){
        for(int j=0; j<inds_size; j++){
            out(i,j) = in(i,inds(j));
        }
    }
}

typedef struct {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    double time;
    Eigen::Vector3f acc;
    Eigen::Vector3f gyr;
} ImuData;

typedef struct {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    double time;
    Eigen::Vector3f acc;
    Eigen::Vector3f gyr;
} ImageData;

typedef struct{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Eigen::Vector3f p_G_I, p_G_I_null;
    Eigen::Vector3f v_G_I, b_g, b_a, g, v_G_I_null;
    Eigen::Quaternionf q_I_G, q_I_G_null;
} ImuState;

typedef struct {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    float c_u, c_v, f_u, f_v, b;
    Eigen::Quaternionf q_C_I;
    Eigen::Vector3f p_I_C;
} Camera;

typedef struct {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    float u_var, v_var;
    Eigen::Matrix<float, 12, 12> Q_imu;
    Eigen::Matrix<float, 15, 15> init_imu_covar;

} NoiseParams;

typedef struct {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    float max_gn_cost_norm, min_rcond, translation_threshold;
    float redundancy_angle_thresh, redundancy_distance_thresh;
    int min_track_length, max_track_length, max_cam_states;

} MSCKFParams;

typedef struct {
    size_t feature_id;
    Eigen::Vector3f point;
    bool is_init = false;

    std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>> observations;
    std::vector<size_t> cam_state_ids;
} FeatureTrack;

typedef struct {
    double time;
    Eigen::Quaternionf q_C_G;
    Eigen::Vector3f p_G_C;
    std::vector<size_t> observed_feature_ids;
    int state_id = -1;
} CameraState;

typedef struct {
    size_t feature_id;
    Eigen::Vector3f point;
    bool is_init = false;

    std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>> observations;
    std::vector<size_t> cam_state_container_indexes;
    std::vector<CameraState> cam_states;

} FeatureTrackToResidualize;



#endif //MSCKF_MONO_DATA_TYPES_H
